// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingMetricsControllerAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Navigation
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.IO;
    using Microsoft.Robotics.Navigation.Benchmark;
    using Microsoft.Robotics.Navigation.Localization;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime;
    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;
    using Test.Robotics.Metrics;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;    

    /// <summary>
    /// Path planning and tracking metrics controller agent.
    /// </summary>
    [DataContract]
    public class PathPlanningAndTrackingMetricsControllerAgent 
        : ConsumerProducerAgent<SimpleAgentMessage<DriveDirection>, SimpleAgentMessage<DriveDirection>>,
        ISubscriptionReceiver<SimpleAgentMessage<PathPlanningAndTrackingMetrics>>,
        ISubscriptionReceiver<BatteryMessage>,
        ISubscriptionReceiver<SimpleAgentMessage<DriveFeedbackState>>,
        ISubscriptionReceiver<RobotPoseEstimationAgentMessage>,
        IMessagePublisher<CompletionMessage>,
        IMessagePublisher<MetricsResultMessage>
    {
        /// <summary>
        /// Maximum time expected to acquire pose estimation message.
        /// </summary>
        /// <remarks>
        /// If exceeded, we assume a control message to the camera may have been
        /// dropped and resend.
        /// </remarks>
        private const long MaxMillisecondExpectedForPoseEstimation = 25000;

        /// <summary>
        /// Tracing context unique to this class.
        /// </summary>
        private static TraceContext traceCtxt = TraceContexts.FromString(TraceContexts.Test, "OnlineMetrics");

        /// <summary>
        /// Name of the agent producing metrics run progress messages.
        /// </summary>
        [DataMember(Name = "MetricsProducer")]
        private readonly string metricsProducer = default(string);

        /// <summary>
        /// Name of the agent producing battery messages.
        /// </summary>
        [DataMember(Name = "BatteryProducer")]
        private readonly string batteryProducer = default(string);

        /// <summary>
        /// Name of the agent producing differential dive messages.
        /// </summary>
        [DataMember(Name = "DriveFeedbackProducer")]
        private readonly string driveFeedbackProducer = default(string);

        /// <summary>
        /// Name of the agent producing camera pose estimation messages.
        /// </summary>
        [DataMember(Name = "CameraPoseEstimationProducer")]
        private readonly string cameraPoseEstimationProducer = default(string);

        /// <summary>
        /// Name of the agent producing robot pose estimation messages.
        /// </summary>
        [DataMember(Name = "RobotPoseEstimationProducer")]
        private readonly string robotPoseEstimationProducer = default(string);

        /// <summary>
        /// Name of the agent consuming RGB camera control messages.
        /// </summary>
        [DataMember(Name = "RGBCameraControl")]
        private readonly string rgbCameraControl = default(string);

        /// <summary>
        /// Name of the agent consuming turret scanner control messages.
        /// </summary>
        [DataMember(Name = "TurretScannerControl")]
        private readonly string turretScannerControl = default(string);

        /// <summary>
        /// Minimum number of ground truth samples taken at each waypoint.
        /// </summary>
        [DataMember(Name = "MinimumGroundTruthSamplesPerWaypoint")]
        private readonly int minimumGroundTruthSamplesPerWaypoint;

        /// <summary>
        /// Threshold of agreement between ground truth samples.
        /// </summary>
        [DataMember(Name = "GroundTruthSampleConsensusThresholdInMillimeters")]
        private readonly double groundTruthSampleConsensusThresholdInMillimeters;

        /// <summary>
        /// Threshold below which we need to dock and charge.
        /// </summary>
        [DataMember(Name = "BatteryLowThreshold")]
        private readonly double batteryLowThreshold;

        /// <summary>
        /// Threshold at which we consider to have arrived at a waypoint.
        /// </summary>
        [DataMember(Name = "WaypointArrivalThreshold")]
        private readonly double waypointArrivalThreshold;

        /// <summary>
        /// Set of one or more landmarks to visit.
        /// </summary>
        [DataMember(Name = "LandmarkIds")]
        private readonly int[] landmarkIds;

        /// <summary>
        /// The navWaypoints file that was generated from SLAM and ground truth.
        /// </summary>
        [DataMember(Name = "NavWaypointsFileName")]
        private readonly string navWaypointsFileName;

        /// <summary>
        /// Data file to which CSV localization metrics data is stored.
        /// </summary>
        [DataMember(Name = "LocalizationMetricsLogFilePath")]
        private readonly string localizationMetricsLogFilePath;

        /// <summary>
        /// Data file to which CSV navigation metrics data is stored.
        /// </summary>
        [DataMember(Name = "NavMetricsLogFilePath")]
        private readonly string navMetricsLogFilePath;

        /// <summary>
        /// Flag indicating whether to make ground truth measurements.
        /// </summary>
        [DataMember(Name = "MeasureGroundTruth")]
        private readonly bool measureGroundTruth;

        /// <summary>
        /// Map resolution in meters.
        /// </summary>
        [DataMember(Name = "MapResolution")]
        private readonly double mapResolution;

        /// <summary>
        /// Name of the robot being controlled for this metrics run.
        /// </summary>
        [DataMember(Name = "RobotName")]
        private readonly string robotName;

        /// <summary>
        /// Hostname of the benchmark server (optional).
        /// </summary>
        [DataMember(Name = "BenchmarkServerHostname")]
        private readonly string benchmarkServerHostname;

        /// <summary>
        /// Port of the benchmark server.
        /// </summary>
        [DataMember(Name = "BenchmarkServerPort")]
        private readonly int benchmarkServerPort;

        /// <summary>
        /// The starting pose of the robot
        /// </summary>
        [DataMember(Name = "StartPose")]
        private readonly Pose2D startPose;

        /// <summary>
        /// The landmark ID where the benchmark starts
        /// </summary>
        [DataMember(Name = "BenchmarkStartingLandmarkId")]
        private readonly int benchmarkStartingLandmarkId;

        /// <summary>
        /// Nav benchmark client for reporting metrics.
        /// </summary>
        private NavBenchmarkClient benchmarkClient;

        /// <summary>
        /// Agent producing metrics run progress messages.
        /// </summary>
        private IAgentHost metricsAgent;

        /// <summary>
        /// Agent producing battery messages.
        /// </summary>
        private IAgentHost batteryAgent;

        /// <summary>
        /// Agent producing drive feedback drive messages.
        /// </summary>
        private IAgentHost driveFeedbackAgent;

        /// <summary>
        /// Agent producing camera pose estimation messages.
        /// </summary>
        private IAgentHost cameraPoseEstimationAgent;

        /// <summary>
        /// Agent producing robot pose estimation messages.
        /// </summary>
        private IAgentHost robotPoseEstimationAgent;

        /// <summary>
        /// Agent producing RGB camera messages.
        /// </summary>
        private IAgentHost rgbCameraAgent;

        /// <summary>
        /// Agent consuming turret scan messages.
        /// </summary>
        private IAgentHost turretScannerAgent;

        /// <summary>
        /// Camera Pose Estimation Input Forwarder used to pass landmark ID arrival.
        /// </summary>
        private IForwarder<CameraPoseEstimationInputMessage> cameraPoseEstimationInputForwarder;

        /// <summary>
        /// RGB camera input forwarder used to enable/disable camera.
        /// </summary>
        private IForwarder<RGBCameraInputMessage> rgbCameraInputForwarder;

        /// <summary>
        /// Turret scanner input forwarder used to initiate a turret scan.
        /// </summary>
        private IForwarder<TurretScanInitiationMessage> turretScannerInputForwarder;

        /// <summary>
        /// State machine controlling behavior for metrics runs.
        /// </summary>
        private PathPlanningAndTrackingMetricsController controller;

        /// <summary>
        /// Last timestamp.
        /// </summary>
        /// <remarks>Used to determine elapsed time for controller updates.</remarks>
        private DateTime lastTimestamp;

        /// <summary>
        /// Last status message written to trace output (used to prevent redundant messages).
        /// </summary>
        private string lastTracedStatus;

        /// <summary>
        /// A forwarder that can be used to broadcast completion messages.
        /// </summary>
        private IForwarder<CompletionMessage> completionForwarder;

        /// <summary>
        /// A forwarder that can be used to broadcast metrics results messages.
        /// </summary>
        private IForwarder<MetricsResultMessage> resultsForwarder;

        /// <summary>
        /// Identifier for the particular metrics run.
        /// </summary>
        /// <remarks>Based on the timestamp at the beginning of the run.</remarks>
        private long runId;

        /// <summary>
        /// Indication of whether currently expecting pose estimation messages.
        /// </summary>
        private bool poseEstimationExpected;

        /// <summary>
        /// Whether pose estimation has been received since turning the camera on.
        /// </summary>
        private bool poseEstimationReceived;

        /// <summary>
        /// Timestamp of last pose estimation message.
        /// </summary>
        private long lastPoseEstimationTimestamp;

        /// <summary>
        /// List of waypoints generated from the list of landmarkIds looked up in the navWaypoints file
        /// </summary>
        private List<Waypoint> waypoints;

        /// <summary>
        /// The ID of the last landmark visited
        /// </summary>
        private int lastLandmarkId;

        /// <summary>
        /// Initializes a new instance of the PathPlanningAndTrackingMetricsControllerAgent class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="localizationMetricsLogFilePath">Data file to which CSV localization metrics data is stored.</param>
        /// <param name="navMetricsLogFilePath">Data file to which CSV navigation metrics data is stored.</param>
        /// <param name="minimumRobotPoseSamplesPerWaypoint">Minimum number of ground truth samples taken at each waypoint.</param>
        /// <param name="robotPoseSampleConsensusThresholdInMillimeters">Threshold of agreement between ground truth samples.</param>
        /// <param name="batteryLowThreshold">Threshold below which we need to dock and charge.</param>
        /// <param name="waypointArrivalThreshold">Threshold at which we consider to have arrived at a waypoint.</param>
        /// <param name="mapResolution">Map resolution in meters.</param>
        /// <param name="navWaypointsFileName">The path to the waypoints file</param>
        /// <param name="landmarkIds">Set of one or more landmarks to visit.</param>
        /// <param name="startPose">The starting pose of the robot</param>
        /// <param name="measureGroundTruth">Flag indicating whether to take ground truth measurements.</param>
        /// <param name="robotName">Name of the robot being controlled for this metrics run.</param>
        /// <param name="driveDirectionProducer">Name of agent producing path plan messages.</param>
        /// <param name="metricsProducer">Name of agent producing metrics run progress messages.</param>
        /// <param name="batteryProducer">Name of agent producing battery messages.</param>
        /// <param name="driveFeedbackProducer">Name of agent producing drive feedback messages.</param>
        /// <param name="cameraPoseEstimationProducer">Name of agent producing camera pose estimation messages.</param>
        /// <param name="robotPoseEstimationProducer">Name of agent producing robot pose estimation messages.</param>
        /// <param name="rgbCameraControl">Name of agent accepting RGB camera messages.</param>
        /// <param name="turretScannerControl">Name of agent accepting turret control messages.</param>
        /// <param name="benchmarkServerHostname">Hostname of benchmark server (optional).</param>
        /// <param name="benchmarkServerPort">Port number of benchmark server.</param>
        /// <param name="benchmarkStartingLandmarkId">The starting landmark ID during NavBenchmark runs.  Can be zero for other metrics</param>
        public PathPlanningAndTrackingMetricsControllerAgent(
            string name,
            string localizationMetricsLogFilePath,
            string navMetricsLogFilePath,
            int minimumRobotPoseSamplesPerWaypoint,
            double robotPoseSampleConsensusThresholdInMillimeters,
            double batteryLowThreshold,
            double waypointArrivalThreshold,
            double mapResolution,
            string navWaypointsFileName,
            int[] landmarkIds,
            Pose2D startPose,
            bool measureGroundTruth,
            string robotName,
            IProducer<SimpleAgentMessage<DriveDirection>> driveDirectionProducer,
            IProducer<SimpleAgentMessage<PathPlanningAndTrackingMetrics>> metricsProducer,
            IProducer<BatteryMessage> batteryProducer,
            IProducer<SimpleAgentMessage<DriveFeedbackState>> driveFeedbackProducer,
            IProducer<CameraPoseEstimationAgentMessage> cameraPoseEstimationProducer,
            IProducer<RobotPoseEstimationAgentMessage> robotPoseEstimationProducer,
            IControllable<RGBCameraInputMessage> rgbCameraControl,
            IControllable<TurretScanInitiationMessage> turretScannerControl,
            string benchmarkServerHostname,
            int benchmarkServerPort,
            int benchmarkStartingLandmarkId)
            : base(name, driveDirectionProducer.Name)
        {
            if (landmarkIds == null || landmarkIds.Length == 0)
            {
                throw new ArgumentException("Goal landmark IDs not set.");
            }

            this.localizationMetricsLogFilePath = localizationMetricsLogFilePath;
            this.navMetricsLogFilePath = navMetricsLogFilePath;
            this.minimumGroundTruthSamplesPerWaypoint = minimumRobotPoseSamplesPerWaypoint;
            this.groundTruthSampleConsensusThresholdInMillimeters = robotPoseSampleConsensusThresholdInMillimeters;
            this.batteryLowThreshold = batteryLowThreshold;
            this.waypointArrivalThreshold = waypointArrivalThreshold;
            this.mapResolution = mapResolution;
            this.landmarkIds = landmarkIds;
            this.startPose = startPose;
            this.measureGroundTruth = measureGroundTruth;
            this.robotName = robotName;
            this.metricsProducer = metricsProducer.Name;
            this.batteryProducer = batteryProducer.Name;
            this.driveFeedbackProducer = driveFeedbackProducer.Name;
            this.cameraPoseEstimationProducer = cameraPoseEstimationProducer.Name;
            this.robotPoseEstimationProducer = robotPoseEstimationProducer.Name;
            this.rgbCameraControl = rgbCameraControl.Name;
            this.turretScannerControl = turretScannerControl.Name;
            this.benchmarkServerHostname = benchmarkServerHostname;
            this.benchmarkServerPort = benchmarkServerPort;
            this.navWaypointsFileName = navWaypointsFileName;
            this.benchmarkStartingLandmarkId = benchmarkStartingLandmarkId;
        }

        /// <summary>
        /// Called to give the implementing agent a forwarder that it can use to publish messages.
        /// </summary>
        /// <param name="forwarder">The forwarder that can be used to publish messages of the right type.</param>
        public void InitializePublisher(IForwarder<CompletionMessage> forwarder)
        {
            this.completionForwarder = forwarder;
        }

        /// <summary>
        /// Called to give the implementing agent a forwarder that it can use to publish messages.
        /// </summary>
        /// <param name="forwarder">The forwarder that can be used to publish messages of the right type.</param>
        public void InitializePublisher(IForwarder<MetricsResultMessage> forwarder)
        {
            this.resultsForwarder = forwarder;
        }

        /// <summary>
        /// Called when agent is initialized.
        /// </summary>
        /// <param name="locator">Agent locator.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);            

            this.metricsAgent = locator.GetAgent(this.metricsProducer);
            this.metricsAgent.Subscribe<SimpleAgentMessage<PathPlanningAndTrackingMetrics>>(this); // Activated below in Initial state
            this.batteryAgent = locator.GetAgent(this.batteryProducer);
            this.batteryAgent.Subscribe<BatteryMessage>(this);
            this.driveFeedbackAgent = locator.GetAgent(this.driveFeedbackProducer);
            this.driveFeedbackAgent.Subscribe<SimpleAgentMessage<DriveFeedbackState>>(this);
            this.cameraPoseEstimationAgent = locator.GetAgent(this.cameraPoseEstimationProducer);
            this.cameraPoseEstimationInputForwarder = this.cameraPoseEstimationAgent.GetControlPort<CameraPoseEstimationInputMessage>();
            this.robotPoseEstimationAgent = locator.GetAgent(this.robotPoseEstimationProducer);
            this.robotPoseEstimationAgent.Subscribe<RobotPoseEstimationAgentMessage>(this);
            this.rgbCameraAgent = locator.GetAgent(this.rgbCameraControl);
            this.rgbCameraInputForwarder = this.rgbCameraAgent.GetControlPort<RGBCameraInputMessage>();
            this.turretScannerAgent = locator.GetAgent(this.turretScannerControl);
            this.turretScannerInputForwarder = this.turretScannerAgent.GetControlPort<TurretScanInitiationMessage>();
            this.lastTimestamp = DateTime.Now;

            List<Waypoint> availableWaypoints = DataLoader<List<Waypoint>>.ImportData(this.navWaypointsFileName);
            this.waypoints = new List<Waypoint>();

            foreach (int i in this.landmarkIds)
            {
                Waypoint wpt = availableWaypoints.First<Waypoint>(w => w.LandmarkId == i);

                // convert to map coordinates
                wpt = new Waypoint(new Pose2D(wpt.Pose.X / this.mapResolution, wpt.Pose.Y / this.mapResolution, wpt.Pose.Heading), wpt.RelativeToLandmark, wpt.IsMarked, wpt.LandmarkId, wpt.IsDock);
                this.waypoints.Add(wpt);
            }

            if (this.benchmarkServerHostname != null && this.benchmarkServerHostname.Length > 0)
            {
                this.benchmarkClient = new NavBenchmarkClient(this.benchmarkServerHostname, this.benchmarkServerPort);
            } 
            
            this.controller = new PathPlanningAndTrackingMetricsController(
                this.minimumGroundTruthSamplesPerWaypoint,
                this.groundTruthSampleConsensusThresholdInMillimeters,
                this.batteryLowThreshold,
                this.waypointArrivalThreshold,
                this.waypoints.ToArray(),
                this.measureGroundTruth,
                this.mapResolution,
                this.benchmarkServerHostname != null);
            
            this.SetCameraState(false);

            this.lastLandmarkId = this.benchmarkStartingLandmarkId;
        }

        /// <summary>
        /// Called when the agent is activated.
        /// </summary>
        public override void OnActivated()
        {
            // note: metricsAgent activated on demand
            base.OnActivated();
            this.batteryAgent.Activate();
            this.driveFeedbackAgent.Activate();
            this.cameraPoseEstimationAgent.Activate();
            this.robotPoseEstimationAgent.Activate();
            this.rgbCameraAgent.Activate();
            this.turretScannerAgent.Activate();
            if (!Console.IsOutputRedirected)
            {
                Console.Clear();
            }

            this.SetCameraState(false);
        }

        /// <summary>
        /// Called when the agent is deactivated.
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.batteryAgent.Deactivate();
            this.driveFeedbackAgent.Deactivate();
            this.cameraPoseEstimationAgent.Deactivate();
            this.robotPoseEstimationAgent.Deactivate();
            this.rgbCameraAgent.Deactivate();
            this.turretScannerAgent.Deactivate();
            if (this.metricsAgent.IsActive)
            {
                this.metricsAgent.Deactivate();
            }
        }

        /// <summary>
        /// Called when the agent receives a message
        /// </summary>
        /// <param name="message">The received message</param>
        public override void Receive(SimpleAgentMessage<DriveDirection> message)
        {
            this.ProcessCommands(this.controller.UpdateDrive(message.State, DateTime.Now), message);
        }

        /// <summary>
        /// Called when the agent receives a message.
        /// </summary>
        /// <param name="message">The received message.</param>
        public void Receive(SimpleAgentMessage<PathPlanningAndTrackingMetrics> message)
        {
            this.ProcessCommands(this.controller.UpdateNavMetrics(message.State, DateTime.Now), message);
        }

        /// <summary>
        /// Called when the agent receives a message.
        /// </summary>
        /// <param name="message">The received message.</param>
        public void Receive(BatteryMessage message)
        {
            BatteryState battery = message.State[0];
            this.ProcessCommands(this.controller.UpdateBattery(battery.BatteryCharging, battery.ChargePercentage, DateTime.Now), message);
        }

        /// <summary>
        /// Called when the agent receives a differential drive message
        /// </summary>
        /// <param name="message">The drive feedback message</param>
        public void Receive(SimpleAgentMessage<DriveFeedbackState> message)
        {
            this.ProcessCommands(this.controller.UpdateDistance(message.State), message);
        }

        /// <summary>
        /// Called when the agent receives a message.
        /// </summary>
        /// <param name="message">The received message.</param>
        public void Receive(RobotPoseEstimationAgentMessage message)
        {
            if (!this.poseEstimationExpected)
            {
                TraceOut.Info(traceCtxt, "Resending camera stop.");
                this.SetCameraState(false); // camera must have missed the message?
                return;
            }

            this.poseEstimationReceived = true;
            this.lastPoseEstimationTimestamp = message.Timestamp;

            Landmark landmark = message.Landmark;
            Pose2D? robotPose = message.RobotPose;
            Pose2D robotRelativeToWaypoint;

            if (landmark != null && robotPose != null && robotPose.HasValue)
            {
                Pose2D waypointRelativeToLandmark = this.waypoints.Where((w) => w.LandmarkId == landmark.Id).First().RelativeToLandmark;
                Pose2D robotRelativeToLandmark = robotPose.Value - landmark.Pose;
                robotRelativeToWaypoint = robotRelativeToLandmark - waypointRelativeToLandmark;

                TraceOut.Info(traceCtxt, "Landmark: ID={0} X={1} Y={2} Theta={3}", landmark.Id, landmark.Pose.X, landmark.Pose.Y, landmark.Pose.Heading);
                TraceOut.Info(traceCtxt, "RobotPose: X={0} Y={1} Theta={2}", robotPose.Value.X, robotPose.Value.Y, robotPose.Value.Heading);
            }
            else
            {
                robotRelativeToWaypoint = new Pose2D(double.NaN, double.NaN, double.NaN);
            }

            this.ProcessCommands(this.controller.UpdateLocMetrics(robotRelativeToWaypoint), message);
        }

        /// <summary>
        /// Start (or restart) metrics run.
        /// </summary>
        /// <param name="runId">Identifier for the particular metrics run.</param>
        private void StartMetrics(long runId)
        {
            if (this.metricsAgent.IsActive)
            {
                this.metricsAgent.Deactivate();
            }

            this.metricsAgent.Activate();
            this.runId = runId;
        }

        /// <summary>
        /// Emit and log status message updates.
        /// </summary>
        /// <param name="message">Status message.</param>
        private void Status(string message)
        {
            if (!Console.IsOutputRedirected)
            {
                Console.SetCursorPosition(0, 0);
                Console.WriteLine("{0,-50}", message);
            }

            if (message != this.lastTracedStatus)
            {
                this.lastTracedStatus = message;
                TraceOut.Info(traceCtxt, "{0} {1:u}", message, DateTime.Now);
            }
        }

        /// <summary>
        /// Cause drive commands to be issued.
        /// </summary>
        /// <param name="direction">Drive direction.</param>
        /// <param name="originatingMessage">The message that triggered the creation of this one.</param>
        private void Drive(DriveDirection direction, AgentMessage originatingMessage)
        {
            this.Publisher.Post(new SimpleAgentMessage<DriveDirection>(direction, originatingMessage));
        }

        /// <summary>
        /// Relay next waypoint to path tracking.
        /// </summary>
        /// <param name="target">Target waypoint.</param>
        private void SetTarget(Pose2D target)
        {
            PathPlanningAgent.Target = target;
            if (this.benchmarkClient != null)
            {
                TraceOut.Info(traceCtxt, "Benchmark client: Signal begin navigation from landmark {0}.", this.lastLandmarkId);
                this.benchmarkClient.SendMessage(this.lastLandmarkId, false);
            }
        }

        /// <summary>
        /// Signal arrival at a waypoint.
        /// </summary>
        /// <param name="waypointIndex">Waypoint index at which we have arrived.</param>
        private void SignalLandmarkArrival(int waypointIndex)
        {
            if (waypointIndex >= 0 && this.waypoints[waypointIndex].IsMarked)
            {
                int landmarkId = this.lastLandmarkId = this.waypoints[waypointIndex].LandmarkId;
                this.cameraPoseEstimationInputForwarder.Post(new CameraPoseEstimationInputMessage(landmarkId));
                if (this.benchmarkClient != null)
                {
                    TraceOut.Info(traceCtxt, "Benchmark client: Sending arrived signal at landmark {0}.", landmarkId);
                    this.benchmarkClient.SendMessage(landmarkId, true); // this blocks until response is received from nav benchmark so robot doesn't move during reading of the landmark
                    TraceOut.Info(traceCtxt, "Benchmark client: got response from server.");
                }
            }
        }
        
        /// <summary>
        /// Enable or disable the RGB camera.
        /// </summary>
        /// <param name="enabled">Whether to enable the camera.</param>
        private void SetCameraState(bool enabled)
        {
            TraceOut.Info(traceCtxt, "Sending Camera Control Message: {0}", enabled);
            this.rgbCameraInputForwarder.Post(new RGBCameraInputMessage(enabled));
        }

        /// <summary>
        /// Initiate turret scan.
        /// </summary>
        /// <param name="originatingMessage">The message that triggered the creation of this one.</param>
        private void InitiateTurretScan(AgentMessage originatingMessage)
        {
            this.Drive(DriveDirection.Stop, originatingMessage);
            this.turretScannerInputForwarder.Post(new TurretScanInitiationMessage());
        }

        /// <summary>
        /// Display intermediate metrics status.
        /// </summary>
        /// <param name="metrics">Metrics information.</param>
        private void MetricsStatus(PathPlanningAndTrackingMetrics metrics)
        {
            if (!Console.IsOutputRedirected)
            {
                Console.SetCursorPosition(0, 2);
                Console.WriteLine("Total Rotation:    {0:0.000}", metrics.TotalRotationalDistance);
                Console.WriteLine("Total Translation: {0:0.000}", metrics.TotalTranslationDistance);
                Console.WriteLine("Total Time:        {0,-50}", metrics.TotalElapsedTime);
                Console.WriteLine();
                Console.WriteLine("Current Deviation: {0:0.000}", metrics.CurrentDeviationFromPath);
                Console.WriteLine("Average Deviation: {0:0.000}", metrics.AverageDeviationFromPath);
                Console.WriteLine("Maximum Deviation: {0:0.000}", metrics.MaximumDeviationFromPath);
                Console.WriteLine();
                Console.WriteLine("Current Pose:      {0:0.000} {1:0.000} {2:0.000}", metrics.CurrentPose.X, metrics.CurrentPose.Y, metrics.CurrentPose.Heading);
                Console.WriteLine("Target Pose:       {0:0.000} {1:0.000} {2:0.000}", PathPlanningAgent.Target.X, PathPlanningAgent.Target.Y, PathPlanningAgent.Target.Heading);
                Console.WriteLine("Proximity:         {0:0.000}", PathPlanningCommon.EuclideanDistance(PathPlanningCommon.PosePoint(metrics.CurrentPose), PathPlanningCommon.PosePoint(PathPlanningAgent.Target)));
            }
        }

        /// <summary>
        /// Translate double.NaN values to double.MaxValue to make SQL happy.
        /// </summary>
        /// <param name="n">Values to be translated.</param>
        /// <returns>Translated value.</returns>
        private double TranslateNaN(double n)
        {
            return double.IsNaN(n) ? double.MaxValue : n;
        }

        /// <summary>
        /// Append content to metrics file.
        /// </summary>
        /// <param name="file">File path.</param>
        /// <param name="content">Content to append.</param>
        private void AppendMetrics(string file, string content)
        {
            string dir = Path.GetDirectoryName(file);
            if (!Directory.Exists(dir))
            {
                Directory.CreateDirectory(dir);
            }

            File.AppendAllText(file, content);
        }

        /// <summary>
        /// Record ground truth metrics for waypoint.
        /// </summary>
        /// <remarks>
        /// Here we are recording the translation and rotational accuracy of localization compared to ground truth.
        /// Ground truth gives us X/Y (and Z) relative to the markers. The waypoints for localization have
        /// been given such that they should be directly under the sign. The Euclidean distance from the
        /// marker is recorded as the translation accuracy.
        /// Ground truth also gives us a heading relative to the markers. The absolute difference between
        /// this and the heading reported by localization is recorded at the rotational accuracy. However,
        /// we are not attempting to achieve a particular heading under each marker.
        /// Additionally logged for debugging is the current pose (in meters) reported by localization.
        /// </remarks>
        /// <param name="relativePose">Robot pose relative to ground truth.</param>
        /// <param name="currentPose">Current pose according to navigation.</param>
        /// <param name="mapDistanceResolution">Map distance resolution.</param>
        /// <param name="waypoint">Waypoint index.</param>
        /// <param name="originatingMessage">The message that triggered the creation of this one.</param>
        private void RecordLocMetrics(Pose2D relativePose, Pose2D currentPose, double mapDistanceResolution, int waypoint, AgentMessage originatingMessage)
        {
            double translationalAccuracy = Math.Sqrt(relativePose.X * relativePose.X + relativePose.Y * relativePose.Y);
            double rotationalAccuracy = Math.Abs(relativePose.Heading - currentPose.Heading);

            string logRelativePose = string.Format(
                "{0:0.000},{1:0.000},{2:0.000},{3:0.000},{4:0.000}",
                relativePose.X,
                relativePose.Y,
                relativePose.Heading,
                translationalAccuracy,
                rotationalAccuracy);

            string logCurrentPose = string.Format(
                "{0:0.000},{1:0.000},{2:0.000}",
                currentPose.X * mapDistanceResolution,
                currentPose.Y * mapDistanceResolution,
                currentPose.Heading);

            string log = string.Format(
                "{0},{1},{2},{3},{4}",
                DateTime.Now.ToString("s"),
                waypoint,
                logRelativePose,
                logCurrentPose,
                Environment.NewLine);

            this.AppendMetrics(this.localizationMetricsLogFilePath, log);

            string robot = "EV4A-007"; // TODO: Where to get this?

            // special negative waypoint values are used to indicate docking (-2) and undocking (-1)
            int landmarkId = waypoint >= 0 ? this.waypoints[waypoint - 1].LandmarkId : waypoint;

            this.resultsForwarder.Post(new MetricsResultMessage(
                type: "Loc Translation",
                groundTruthId: landmarkId,
                accuracy: this.TranslateNaN(translationalAccuracy),
                precision: 0,
                weight: 1,
                debugData: logRelativePose,
                duration: 0,
                robot: robot,
                runId: this.runId,
                originatingMessage: originatingMessage));

            this.resultsForwarder.Post(new MetricsResultMessage(
                type: "Loc Rotation",
                groundTruthId: landmarkId,
                accuracy: this.TranslateNaN(rotationalAccuracy),
                precision: 0,
                weight: 1,
                debugData: logRelativePose,
                duration: 0,
                robot: robot,
                runId: this.runId,
                originatingMessage: originatingMessage));
        }

        /// <summary>
        /// Record final metrics for a completed run.
        /// </summary>
        /// <remarks>
        /// Here we are recording the total rotation and translation as well as stats on how closely we're tracking
        /// during navigation. Wheel odometry is accumulated during each run. We report total rotational and translational
        /// distances as "accuracy" for now. We also report the average and maximum deviation from the path (in meters)
        /// as "accuracy" and "precision".
        /// </remarks>
        /// <param name="metrics">Metrics information.</param>
        /// <param name="dockingAttempts">Number of docking attempt made.</param>
        /// <param name="originatingMessage">The message that triggered the creation of this one.</param>
        private void RecordNavMetrics(PathPlanningAndTrackingMetrics metrics, int dockingAttempts, AgentMessage originatingMessage)
        {
            this.AppendMetrics(
                this.navMetricsLogFilePath,
                string.Format(
                    "{0},{1:0.000},{2:0.000},{3},{4:0.000},{5:0.000},{6}{7}",
                    DateTime.Now.ToString("s"),
                    metrics.TotalRotationalDistance,
                    metrics.TotalTranslationDistance,
                    metrics.TotalElapsedTime.ToString("c"),
                    metrics.AverageDeviationFromPath,
                    metrics.MaximumDeviationFromPath,
                    dockingAttempts,
                    Environment.NewLine));

            this.resultsForwarder.Post(new MetricsResultMessage(
                type: "Nav Tracking",
                groundTruthId: 0,
                accuracy: this.TranslateNaN(metrics.AverageDeviationFromPath),
                precision: this.TranslateNaN(metrics.MaximumDeviationFromPath),
                weight: metrics.TotalTranslationDistance,
                debugData: null,
                duration: (long)metrics.TotalElapsedTime.TotalSeconds,
                robot: this.robotName,
                runId: this.runId,
                originatingMessage: originatingMessage));

            this.resultsForwarder.Post(new MetricsResultMessage(
                type: "Nav Rotation",
                groundTruthId: 0,
                accuracy: this.TranslateNaN(metrics.TotalRotationalDistance),
                precision: 0,
                weight: metrics.TotalTranslationDistance,
                debugData: null,
                duration: (long)metrics.TotalElapsedTime.TotalSeconds,
                robot: this.robotName,
                runId: this.runId,
                originatingMessage: originatingMessage));

            this.resultsForwarder.Post(new MetricsResultMessage(
                type: "Nav Translation",
                groundTruthId: 0,
                accuracy: this.TranslateNaN(metrics.TotalTranslationDistance),
                precision: 0,
                weight: metrics.TotalTranslationDistance,
                debugData: null,
                duration: (long)metrics.TotalElapsedTime.TotalSeconds,
                robot: this.robotName,
                runId: this.runId,
                originatingMessage: originatingMessage));

            this.completionForwarder.Post(new CompletionMessage(this.Name));
        }

        /// <summary>
        /// Process controller status updates; causing side-effecting behavior.
        /// </summary>
        /// <param name="output">Current controller output.</param>
        /// <param name="originatingMessage">The message that triggered the creation of this one.</param>
        private void ProcessCommands(PathPlanningAndTrackingMetricsControllerStatus.StatusOutput output, AgentMessage originatingMessage)
        {
            // start metrics
            if (output.StartCollectingMetrics)
            {
                TraceOut.Info(traceCtxt, "Starting metrics collection.");
                this.StartMetrics(originatingMessage.Timestamp);
            }

            // status message
            if (output.Message != null)
            {
                this.Status(output.Message);
            }

            // drive
            if (output.Drive != null && originatingMessage != null)
            {
                this.Drive(output.Drive, originatingMessage);
            }

            // signal arrival at current waypoint (during benchmark runs, always be sure this will happen before setting of a new target)
            if (output.SignalLandmarkArrival)
            {
                TraceOut.Info(traceCtxt, "Signal landmark arrival waypoint #{0}.", output.WaypointId + 1);
                this.SignalLandmarkArrival(output.WaypointId);

                // setup for landmark measurement if not using the nav benchmark
                if (this.benchmarkClient == null)
                {
                    this.poseEstimationReceived = false;
                    this.lastPoseEstimationTimestamp = originatingMessage.Timestamp;
                    this.SetCameraState(this.poseEstimationExpected = true);
                }
            }

            // set target (always be sure this will happen after signalling arrival)
            if (output.Target.HasValue)
            {
                Pose2D target = output.Target.Value;
                TraceOut.Info(traceCtxt, "Setting target {0}, {1}, {2}.", target.X, target.Y, target.Heading);
                this.SetTarget(target);
            }
            
            // update intermediate metrics status
            if (output.UpdateMetricsStatus)
            {
                this.MetricsStatus(output.NavMetrics);
            }

            // record navigation metrics
            if (output.RecordNavMetrics)
            {
                TraceOut.Info(traceCtxt, "Recording nav metrics.");
                this.RecordNavMetrics(output.NavMetrics, output.DockingAttempts, originatingMessage);
            }

            // record localization metrics
            if (output.RecordLocMetrics)
            {
                TraceOut.Info(traceCtxt, "Recording loc metrics.");
                this.RecordLocMetrics(
                    output.LocRelativePose,
                    output.NavMetrics.CurrentPose,
                    output.NavMetrics.MapDistanceResolution,
                    output.WaypointId,
                    originatingMessage);

                TraceOut.Info(traceCtxt, "Sending camera stop.");
                this.SetCameraState(this.poseEstimationExpected = false);
            }

            // turret scan
            if (output.InitiateTurretScan)
            {
                TraceOut.Info(traceCtxt, "Initiating turret scan.");
                this.InitiateTurretScan(originatingMessage);
            }

            // resend a camera start message (that might have been dropped).  Only if not using nav benchmark
            if (
                this.benchmarkClient == null &&
                this.poseEstimationExpected &&
                !this.poseEstimationReceived &&
                originatingMessage.Timestamp - this.lastPoseEstimationTimestamp > MaxMillisecondExpectedForPoseEstimation)
            {
                TraceOut.Info(traceCtxt, "Resending camera start.");
                this.lastPoseEstimationTimestamp = originatingMessage.Timestamp;
                this.SetCameraState(true);
            }

            if (output.Aborted)
            {
                TraceOut.Info(traceCtxt, "Sending completion as aborted.");
                this.completionForwarder.Post(new CompletionMessage(this.Name, true, output.Message));
            }

            output.Reset();
        }
    }
}
